/*
 * RVOSimulator.h
 * RVO2 Library
 *
 * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <https://gamma.cs.unc.edu/RVO2/>
 */

import { Agent } from "./Agent";
import { KdTree } from "./KdTree";
import { Obstacle, RvoPolygon } from "./Obstacle";
import { RVOArray } from "./RVOArrayEx";
import { Vector2 } from "./Vector2";

/**
 * @brief Defines the simulation. The main class of the library that contains
 *        all simulation functionality.
 */
const RVO_ERROR = Number.MAX_SAFE_INTEGER;

const _v4: Vector2 = new Vector2;
const _v5: Vector2 = new Vector2;
const _v6: Vector2 = new Vector2;

export class RVOSimulator {
  private static _instance: RVOSimulator = null;
  public static getInstance(): RVOSimulator {
    return this._instance || (this._instance = new RVOSimulator());
  }

  agents_: RVOArray<Agent>;
  obstacles_: RVOArray<Obstacle>;
  rvoPolygon: RVOArray<RvoPolygon>;
  defaultAgent_: Agent;
  kdTree_: KdTree;
  globalTime_: number;
  timeStep_: number;

  /**
   * @brief     Constructs a simulator instance and sets the default
   *            properties for any new agent that is added.
   * @param[in] timeStep        The time step of the simulation. Must be
   *                            positive.
   * @param[in] neighborDist    The default maximum distance center-point to
   *                            center-point to other agents a new agent takes
   *                            into account in the navigation. The larger this
   *                            number, the longer he running time of the
   *                            simulation. If the number is too low, the
   *                            simulation will not be safe. Must be
   *                            non-negative.
   * @param[in] maxNeighbors    The default maximum number of other agents a
   *                            new agent takes into account in the navigation.
   *                            The larger this number, the longer the running
   *                            time of the simulation. If the number is too
   *                            low, the simulation will not be safe.
   * @param[in] timeHorizon     The default minimal amount of time for which a
   *                            new agent's velocities that are computed by the
   *                            simulation are safe with respect to other
   *                            agents. The larger this number, the sooner an
   *                            agent will respond to the presence of other
   *                            agents, but the less freedom the agent  has in
   *                            choosing its velocities. Must be positive.
   * @param[in] timeHorizonObst The default minimal amount of time for which a
   *                            new agent's velocities that are computed by the
   *                            simulation are safe with respect to obstacles.
   *                            The larger this number, the sooner an agent will
   *                            respond to the presence of obstacles, but the
   *                            less freedom the agent has in choosing its
   *                            velocities. Must be positive.
   * @param[in] radius          The default radius of a new agent. Must be
   *                            non-negative.
   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
   *                            be non-negative.
   * @param[in] velocity        The default initial two-dimensional linear
   *                            velocity of a new agent.
   */
  constructor(
    timeStep: number = 0,
    neighborDist: number = 0,
    maxNeighbors: number = 10,
    timeHorizon: number = 2,
    timeHorizonObst: number = 2,
    radius: number = 0,
    maxSpeed: number = 0,
    velocity?: Vector2
  ) {
    this.defaultAgent_ = new Agent();
    this.kdTree_ = new KdTree(this);
    this.globalTime_ = 0;
    this.timeStep_ = timeStep;

    this.agents_ = new RVOArray();
    this.obstacles_ = new RVOArray();
    this.rvoPolygon = new RVOArray();

    this.defaultAgent_.maxNeighbors_ = maxNeighbors;
    this.defaultAgent_.maxSpeed_ = maxSpeed;
    this.defaultAgent_.neighborDist_ = neighborDist;
    this.defaultAgent_.radius_ = radius;
    this.defaultAgent_.timeHorizon_ = timeHorizon;
    this.defaultAgent_.timeHorizonObst_ = timeHorizonObst;

    if (velocity) {
      this.defaultAgent_.velocity_.set(velocity);
    }
  }

  /**
   * @brief     Adds a new agent to the simulation.
   * @param[in] position        The two-dimensional starting position of this
   *                            agent.
   * @param[in] neighborDist    The maximum distance center-point to
   *                            center-point to other agents this agent takes
   *                            into account in the navigation. The larger this
   *                            number, the longer the running time of the
   *                            simulation. If the number is too low, the
   *                            simulation will not be safe. Must be
   *                            non-negative.
   * @param[in] maxNeighbors    The maximum number of other agents this agent
   *                            takes into account in the navigation. The larger
   *                            this number, the longer the running time of the
   *                            simulation. If the number is too low, the
   *                            simulation will not be safe.
   * @param[in] timeHorizon     The minimal amount of time for which this
   *                            agent's velocities that are computed by the
   *                            simulation are safe with respect to other
   *                            agents. The larger this number, the sooner this
   *                            agent will respond to the presence of other
   *                            agents, but the less freedom this agent has in
   *                            choosing its velocities. Must be positive.
   * @param[in] timeHorizonObst The minimal amount of time for which this
   *                            agent's velocities that are computed by the
   *                            simulation are safe with respect to obstacles.
   *                            The larger this number, the sooner this agent
   *                            will respond to the presence of obstacles, but
   *                            the less freedom this agent has in choosing its
   *                            velocities. Must be positive.
   * @param[in] radius          The radius of this agent. Must be non-negative.
   * @param[in] maxSpeed        The maximum speed of this agent. Must be
   *                            non-negative.
   * @param[in] velocity        The initial two-dimensional linear velocity of
   *                            this agent.
   * @return    The number of the agent.
   */
  public addAgent(
    position: Vector2,
    radius: number = this.defaultAgent_.radius_,
    maxSpeed: number = this.defaultAgent_.maxSpeed_,
    neighborDist: number = this.defaultAgent_.neighborDist_,
    velocity: Vector2 = this.defaultAgent_.velocity_,
    maxNeighbors: number = this.defaultAgent_.maxNeighbors_,
    timeHorizon: number = this.defaultAgent_.timeHorizon_,
    timeHorizonObst: number = this.defaultAgent_.timeHorizonObst_
  ) {
    let agent = Agent.pool.get();
    agent.position_.set(position);
    agent.velocity_.set(velocity);
    agent.maxNeighbors_ = maxNeighbors;
    agent.maxSpeed_ = maxSpeed;
    agent.neighborDist_ = neighborDist;
    agent.radius_ = radius;
    agent.timeHorizon_ = timeHorizon;
    agent.timeHorizonObst_ = timeHorizonObst;
    this.agents_.push(agent);
    this.kdTree_.isAgentsTreeDirty = true;
    return agent;
  }

  public removeAgent(agent: Agent) {
    this.agents_.remove(agent.rvoIndex);
    Agent.pool.put(agent);
    this.kdTree_.isAgentsTreeDirty = true;
  }

  /**
   * 添加障碍
   * @brief     Adds a new obstacle to the simulation.
   * @param[in] vertices List of the vertices of the polygonal obstacle in
   *                     counterclockwise order.
   * @return    The number of the first vertex of the obstacle, or
   *            RVO::RVO_ERROR when the number of vertices is less than two.
   * @note      To add a "negative" obstacle, e.g., a bounding polygon around
   *            the environment, the vertices should be listed in clockwise
   *            order.
   */
  public addObstacle(vertices: Array<Vector2>, withPolygon = true) {
    let obstacleNo = this.obstacles_.length;

    for (let i = 0; i < vertices.length; ++i) {
      let obstacle = Obstacle.pool.get();
      obstacle.point_.set(vertices[i]);

      if (i != 0) {
        obstacle.previous_ = this.obstacles_.get(this.obstacles_.length - 1);
        obstacle.previous_.next_ = obstacle;
      }

      if (i == vertices.length - 1) {
        obstacle.next_ = this.obstacles_.get(obstacleNo);
        obstacle.next_.previous_ = obstacle;
      }

      vertices[(i == vertices.length - 1 ? 0 : i + 1)].sub(vertices[i], _v4).normalize();
      obstacle.direction_.set(_v4);

      if (vertices.length == 2) {
        obstacle.isConvex_ = true;
      } else {
        obstacle.isConvex_ =
          Vector2.leftOf(vertices[i == 0 ? vertices.length - 1 : i - 1],
            vertices[i],
            vertices[i == vertices.length - 1 ? 0 : i + 1]) >= 0;
      }

      obstacle.id_ = this.obstacles_.length;

      this.obstacles_.push(obstacle);
    }

    let polygon: RvoPolygon;
    if (withPolygon && vertices.length) {
      this.kdTree_.isObstacleTreeDirty = true;
      polygon = RvoPolygon.pool.get();
      polygon.vertices = vertices;
      this.rvoPolygon.push(polygon);
    }
    return polygon;
  }

  removeObstacle(polygon: RvoPolygon) {
    this.rvoPolygon.remove(polygon.rvoIndex);
    RvoPolygon.pool.put(polygon);
    this.kdTree_.isObstacleTreeDirty = true;
  }

  /**
   * @brief Lets the simulator perform a simulation step and updates the
   *        two-dimensional position and two-dimensional velocity of each agent.
   */
  public doStep(dt: number) {
    //BUG: dt为零导致避障计算失效
    if (dt <= 0) return;

    this.setTimeStep(dt);
    this.kdTree_.buildAgentTree();
    this.kdTree_.buildObstacleTree();
    let agent: Agent;
    for (let i = 0; i < this.agents_.length; ++i) {
      agent = this.agents_.get(i);
      agent.computeNeighbors(this.kdTree_);
      agent.computeNewVelocity(dt);
    }

    for (let i = 0; i < this.agents_.length; ++i) {
      this.agents_.get(i).update(dt);
    }

    this.globalTime_ += this.timeStep_;
  }

  /**
   * @brief     Returns the specified agent neighbor of the specified agent.
   * @param[in] agentNo    The number of the agent whose agent neighbor is to be
   *                       retrieved.
   * @param[in] neighborNo The number of the agent neighbor to be retrieved.
   * @return    The number of the neighboring agent.
   */
  // getAgentAgentNeighbor(agentNo: number, neighborNo: number) {
  //   return this.agents_.get(agentNo).agentNeighbors_.get(neighborNo).agent.rvoIndex;
  // }

  /**
   * @brief     Returns the maximum neighbor count of a specified agent.
   * @param[in] agentNo The number of the agent whose maximum neighbor count is
   *                    to be retrieved.
   * @return    The present maximum neighbor count of the agent.
   */
  getAgentMaxNeighbors(agentNo: number) {
    return this.agents_.get(agentNo).maxNeighbors_;
  }

  /**
   * @brief     Returns the maximum speed of a specified agent.
   * @param[in] agentNo The number of the agent whose maximum speed is to be
   *                    retrieved.
   * @return    The present maximum speed of the agent.
   */
  getAgentMaxSpeed(agentNo: number) {
    return this.agents_.get(agentNo).maxSpeed_;
  }

  /**
   * @brief     Returns the maximum neighbor distance of a specified agent.
   * @param[in] agentNo The number of the agent whose maximum neighbor distance
   *                    is to be retrieved.
   * @return    The present maximum neighbor distance of the agent.
   */
  // getAgentNeighborDist(agentNo: number) {
  //   return this.agents_.get(agentNo).neighborDist_;
  // }

  /**
   * @brief     Returns the count of agent neighbors taken into account to
   *            compute the current velocity for the specified agent.
   * @param[in] agentNo The number of the agent whose count of agent neighbors
   *                    is to be retrieved.
   * @return    The count of agent neighbors taken into account to compute the
   *            current velocity for the specified agent.
   */
  // getAgentNumAgentNeighbors(agentNo: number) {
  //   return this.agents_.get(agentNo).agentNeighbors_.length;
  // }

  /**
   * @brief     Returns the count of obstacle neighbors taken into account to
   *            compute the current velocity for the specified agent.
   * @param[in] agentNo The number of the agent whose count of obstacle
   *                    neighbors is to be retrieved.
   * @return    The count of obstacle neighbors taken into account to compute
   *            the current velocity for the specified agent.
   */
  getAgentNumObstacleNeighbors(agentNo: number) {
    return this.agents_.get(agentNo).obstacleNeighbors_.length;
  }

  /**
   * @brief     Returns the count of ORCA constraints used to compute the
   *            current velocity for the specified agent.
   * @param[in] agentNo The number of the agent whose count of ORCA constraints
   *                    is to be retrieved.
   * @return    The count of ORCA constraints used to compute the current
   *            velocity for the specified agent.
   */
  getAgentNumORCALines(agentNo: number) {
    return this.agents_.get(agentNo).orcaLines_.length;
  }

  /**
   * @brief     Returns the specified obstacle neighbor of the specified agent.
   * @param[in] agentNo    The number of the agent whose obstacle neighbor is to
   *                       be retrieved.
   * @param[in] neighborNo The number of the obstacle neighbor to be retrieved.
   * @return    The number of the first vertex of the neighboring obstacle edge.
   */
  getAgentObstacleNeighbor(agentNo: number, neighborNo: number) {
    return this.agents_.get(agentNo).obstacleNeighbors_.get(neighborNo).obstacle.id_;
  }

  /**
   * @brief     Returns the specified ORCA constraint of the specified agent.
   * @param[in] agentNo The number of the agent whose ORCA constraint is to be
   *                    retrieved.
   * @param[in] lineNo  The number of the ORCA constraint to be retrieved.
   * @return    A line representing the specified ORCA constraint.
   * @note      The half-plane to the left of the line is the region of
   *            permissible velocities with respect to the specified ORCA
   *            constraint.
   */
  getAgentORCALine(agentNo: number, lineNo: number) {
    return this.agents_.get(agentNo).orcaLines_.get(lineNo);
  }

  /**
   * @brief     Returns the two-dimensional position of a specified agent.
   * @param[in] agentNo The number of the agent whose two-dimensional position
   *                    is to be retrieved.
   * @return    The present two-dimensional position of the center of the agent.
   */
  getAgentPosition(agentNo: number) {
    return this.agents_.get(agentNo).position_;
  }

  /**
   * @brief     Returns the two-dimensional preferred velocity of a specified
   *            agent.
   * @param[in] agentNo The number of the agent whose two-dimensional preferred
   *                    velocity is to be retrieved.
   * @return    The present two-dimensional preferred velocity of the agent.
   */
  getAgentPrefVelocity(agentNo: number) {
    return this.agents_.get(agentNo).prefVelocity_;
  }

  /**
   * @brief     Returns the radius of a specified agent.
   * @param[in] agentNo The number of the agent whose radius is to be retrieved.
   * @return    The present radius of the agent.
   */
  getAgentRadius(agentNo: number) {
    return this.agents_.get(agentNo).radius_;
  }

  /**
   * @brief     Returns the time horizon of a specified agent.
   * @param[in] agentNo The number of the agent whose time horizon is to be
   *                    retrieved.
   * @return    The present time horizon of the agent.
   */
  getAgentTimeHorizon(agentNo: number) {
    return this.agents_.get(agentNo).timeHorizon_;
  }

  /**
   * @brief     Returns the time horizon with respect to obstacles of a
   *            specified agent.
   * @param[in] agentNo The number of the agent whose time horizon with respect
   *                    to obstacles is to be retrieved.
   * @return    The present time horizon with respect to obstacles of the agent.
   */
  getAgentTimeHorizonObst(agentNo: number) {
    return this.agents_.get(agentNo).timeHorizonObst_;
  }

  /**
   * @brief     Returns the two-dimensional linear velocity of a specified
   *            agent.
   * @param[in] agentNo The number of the agent whose two-dimensional linear
   *                    velocity is to be retrieved.
   * @return    The present two-dimensional linear velocity of the agent.
   */
  getAgentVelocity(agentNo: number) {
    return this.agents_.get(agentNo).velocity_;
  }

  /**
   * @brief  Returns the global time of the simulation.
   * @return The present global time of the simulation (zero initially).
   */
  getGlobalTime() {
    return this.globalTime_;
  }

  /**
   * @brief  Returns the count of agents in the simulation.
   * @return The count of agents in the simulation.
   */
  getAgentsNums() {
    return this.agents_.length;
  }

  public getAgents() {
    return this.agents_;
  }

  public getAgent(id: number) {
    return this.agents_.get(id);
  }

  /**
   * @brief  Returns the count of obstacle vertices in the simulation.
   * @return The count of obstacle vertices in the simulation.
   */
  getNumObstacleVertices() {
    return this.obstacles_.length;
  }

  /**
   * @brief     Returns the two-dimensional position of a specified obstacle
   *            vertex.
   * @param[in] vertexNo The number of the obstacle vertex to be retrieved.
   * @return    The two-dimensional position of the specified obstacle vertex.
   */
  getObstacleVertex(vertexNo: number) {
    return this.obstacles_.get(vertexNo).point_;
  }

  /**
   * @brief     Returns the number of the obstacle vertex succeeding the
   *            specified obstacle vertex in its polygon.
   * @param[in] vertexNo The number of the obstacle vertex whose successor is to
   *                     be retrieved.
   * @return    The number of the obstacle vertex succeeding the specified
   *            obstacle vertex in its polygon.
   */
  getNextObstacleVertexNo(vertexNo: number) {
    return this.obstacles_.get(vertexNo).next_.id_;
  }

  /**
   * @brief     Returns the number of the obstacle vertex preceding the
   *            specified obstacle vertex in its polygon.
   * @param[in] vertexNo The number of the obstacle vertex whose predecessor is
   *                     to be retrieved.
   * @return    The number of the obstacle vertex preceding the specified
   *            obstacle vertex in its polygon.
   */
  getPrevObstacleVertexNo(vertexNo: number) {
    return this.obstacles_.get(vertexNo).previous_.id_;
  }

  /**
   * @brief  Returns the time step of the simulation.
   * @return The present time step of the simulation.
   */
  getTimeStep() {
    return this.timeStep_;
  }

  /**
   * @brief Processes the obstacles that have been added so that they are
   *        accounted for in the simulation.
   * @note  Obstacles added to the simulation after this function has been
   *        called are not accounted for in the simulation.
   */
  processObstacles() {
    this.kdTree_.buildObstacleTree();
  }

  /**
   * @brief     Performs a visibility query between the two specified points
   *            with respect to the obstacles
   * @param[in] point1 The first point of the query.
   * @param[in] point2 The second point of the query.
   * @param[in] radius The minimal distance between the line connecting the two
   *                   points and the obstacles in order for the points to be
   *                   mutually visible. Must be non-negative.
   * @return    A boolean specifying whether the two points are mutually
   *            visible. Returns true when the obstacles have not been
   *            processed.
   */
  queryVisibility(point1: Vector2, point2: Vector2, radius: number = 0) {
    return this.kdTree_.queryVisibility(point1, point2, radius);
  }

  /**
   * @brief     Sets the default properties for any new agent that is added.
   * @param[in] neighborDist    The default maximum distance center-point to
   *                            center-point to other agents a new agent takes
   *                            into account in the navigation. The larger this
   *                            number, the longer he running time of the
   *                            simulation. If the number is too low, the
   *                            simulation will not be safe. Must be
   *                            non-negative.
   * @param[in] maxNeighbors    The default maximum number of other agents a new
   *                            agent takes into account in the navigation. The
   *                            larger this number, the longer the running time
   *                            of the simulation. If the number is too low, the
   *                            simulation will not be safe.
   * @param[in] timeHorizon     The default minimal amount of time for which a
   *                            new agent's velocities that are computed by the
   *                            simulation are safe with respect to other
   *                            agents. The larger this number, the sooner an
   *                            agent will respond to the presence of other
   *                            agents, but the less freedom the agent has in
   *                            choosing its velocities. Must be positive.
   * @param[in] timeHorizonObst The default minimal amount of time for which a
   *                            new agent's velocities that are computed by the
   *                            simulation are safe with respect to obstacles.
   *                            The larger this number, the sooner an agent will
   *                            respond to the presence of obstacles, but the
   *                            less freedom the agent has in choosing its
   *                            velocities. Must be positive.
   * @param[in] radius          The default radius of a new agent. Must be
   *                            non-negative.
   * @param[in] maxSpeed        The default maximum speed of a new agent. Must
   *                            be non-negative.
   * @param[in] velocity        The default initial two-dimensional linear
   *                            velocity of a new agent.
   */
  setAgentDefaults(neighborDist: number, maxNeighbors: number, timeHorizon: number,
    timeHorizonObst: number, radius: number, maxSpeed: number, velocity: Vector2 = new Vector2) {
    if (!this.defaultAgent_) {
      this.defaultAgent_ = new Agent();
    }

    this.defaultAgent_.maxNeighbors_ = maxNeighbors;
    this.defaultAgent_.maxSpeed_ = maxSpeed;
    this.defaultAgent_.neighborDist_ = neighborDist;
    this.defaultAgent_.radius_ = radius;
    this.defaultAgent_.timeHorizon_ = timeHorizon;
    this.defaultAgent_.timeHorizonObst_ = timeHorizonObst;
    this.defaultAgent_.velocity_.set(velocity);
  }

  /**
   * @brief     Sets the maximum neighbor count of a specified agent.
   * @param[in] agentNo      The number of the agent whose maximum neighbor
   *                         count is to be modified.
   * @param[in] maxNeighbors The replacement maximum neighbor count.
   */
  setAgentMaxNeighbors(agentNo: number, maxNeighbors: number) {
    this.agents_.get(agentNo).maxNeighbors_ = maxNeighbors;
  }

  /**
   * @brief     Sets the maximum speed of a specified agent.
   * @param[in] agentNo  The number of the agent whose maximum speed is to be
   *                     modified.
   * @param[in] maxSpeed The replacement maximum speed. Must be non-negative.
   */
  setAgentMaxSpeed(agentNo: number, maxSpeed: number) {
    this.agents_.get(agentNo).maxSpeed_ = maxSpeed;
  }

  /**
   * @brief     Sets the maximum neighbor distance of a specified agent.
   * @param[in] agentNo      The number of the agent whose maximum neighbor
   *                         distance is to be modified.
   * @param[in] neighborDist The replacement maximum neighbor distance. Must be
   *                         non-negative.
   */
  setAgentNeighborDist(agentNo: number, neighborDist: number) {
    this.agents_.get(agentNo).neighborDist_ = neighborDist;
  }

  /**
   * @brief     Sets the two-dimensional position of a specified agent.
   * @param[in] agentNo  The number of the agent whose two-dimensional position
   *                     is to be modified.
   * @param[in] position The replacement of the two-dimensional position.
   */
  setAgentPosition(agentNo: number, position: Vector2) {
    this.agents_.get(agentNo).position_.set(position);
  }

  /**
   * @brief     Sets the two-dimensional preferred velocity of a specified
   *            agent.
   * @param[in] agentNo      The number of the agent whose two-dimensional
   *                         preferred velocity is to be modified.
   * @param[in] prefVelocity The replacement of the two-dimensional preferred
   *                         velocity.
   */
  setAgentPrefVelocity(agentNo: number, prefVelocity: Vector2) {
    this.agents_.get(agentNo).prefVelocity_.set(prefVelocity);
  }

  /**
   * @brief     Sets the radius of a specified agent.
   * @param[in] agentNo The number of the agent whose radius is to be modified.
   * @param[in] radius  The replacement radius. Must be non-negative.
   */
  setAgentRadius(agentNo: number, radius: number) {
    this.agents_.get(agentNo).radius_ = radius;
  }

  /**
   * @brief     Sets the time horizon of a specified agent with respect to other
   *            agents.
   * @param[in] agentNo     The number of the agent whose time horizon is to be
   *                        modified.
   * @param[in] timeHorizon The replacement time horizon with respect to other
   *                        agents. Must be positive.
   */
  setAgentTimeHorizon(agentNo: number, timeHorizon: number) {
    this.agents_.get(agentNo).timeHorizon_ = timeHorizon;
  }

  /**
   * @brief     Sets the time horizon of a specified agent with respect to
   *            obstacles.
   * @param[in] agentNo         The number of the agent whose time horizon with
   *                            respect to obstacles is to be modified.
   * @param[in] timeHorizonObst The replacement time horizon with respect to
   *                            obstacles. Must be positive.
   */
  setAgentTimeHorizonObst(agentNo: number, timeHorizonObst: number) {
    this.agents_.get(agentNo).timeHorizonObst_ = timeHorizonObst;
  }

  /**
   * @brief     Sets the two-dimensional linear velocity of a specified agent.
   * @param[in] agentNo  The number of the agent whose two-dimensional linear
   *                     velocity is to be modified.
   * @param[in] velocity The replacement two-dimensional linear velocity.
   */
  setAgentVelocity(agentNo: number, velocity: Vector2) {
    this.agents_.get(agentNo).velocity_.set(velocity);
  }

  /**
   * @brief     Sets the time step of the simulation.
   * @param[in] timeStep The time step of the simulation. Must be positive.
   */
  setTimeStep(timeStep: number) {
    this.timeStep_ = timeStep;
  }
}

